#include <iostream>
#include <thread>
#include <mutex>

#include "src/VideoInput.h"
#include "src/VideoOutput.h"
#include "src/VideoBuffer.h"
#include "src/VideoProcSubSys.h"
#include "src/Func.h"
#include "src/CallerOnce.h"
#include "src/Yolov2.h"
#include "src/LaneDetect.h"

using namespace hiych;

bool stopSignal = false;
bool ret = false;
unsigned int uartID = -1;

std::mutex mtx;
static Yolov2 detectKey("./yolov2_key_detect.wk",4);
static Yolov2 detectNum("./yolov2_num_detect.wk", 10);

template<typename T> void printInfo(T param = T(),bool isNeedPause = true,int line = -1);
template<typename T> void funcInfo(bool ret, T param = T());

void frmMessage(const VideoProcSubSys& vpss,const VideoOutput& vo);

int main()
{
    sdk_init();
    system("cd /sys/class/gpio/;echo 55 > export;echo out > gpio55/direction;echo 1 > gpio55/value");

    VideoInput _vi;
    _vi.setDev();
    _vi.setPipe(0,-1,-1,-1);
    _vi.setChannel(0);

    SIZE_S picSize=getPictureSize(_vi).first;
    PIC_SIZE_E picCate = getPictureSize(_vi).second;

    ret = MIPI_Init(VideoBuffer(picSize));
    funcInfo(ret,"hiych::MIPI_Init");

    int fd = Caller<CALL_ONCE_GET_MIPI_Id>::GET_MIPI_Id();
    printInfo("fd==" + std::to_string(fd), false);

    VideoProcSubSys _vpss;
    _vpss.setGroup(AIC_VPSS_GRP, nullptr, picSize);
    _vpss.setChannel(AIC_VPSS_ZOUT_CHN, nullptr, AICSTART_VI_OUTWIDTH, AICSTART_VI_OUTHEIGHT);

    VideoOutput _vo(picCate);
    ret =  Caller<CALL_ONCE_VI_VPSS_VO_Create>::VI_VPSS_VO_Relate(_vi, _vpss, _vo);
    funcInfo(ret, "CALL_ONCE_VI_VPSS_VO_Create");

    printInfo("loadModel", false, 70);
    detectNum.loadModel();
    detectKey.loadModel();
    uartID = UartOpenInit();
    
    //主线程
    static std::thread mainThread(frmMessage, std::ref(_vpss), std::ref(_vo));
    mainThread.detach();
    printInfo("press enter to exit", true, 81);
    stopSignal = true;

    ret = Caller<CALL_ONCE_CLOSE_MIPI_Id>::CLOSE_MIPI_Id(fd);
    funcInfo(ret ," CALL_ONCE_CLOSE_MIPI_Id ");

    ret = Caller<CALL_ONCE_VI_VPSS_VO_Release>::VI_VPSS_VO_Relate(_vi, _vpss, _vo);
    funcInfo(ret, "CALL_ONCE_VI_VPSS_VO_Release");

    ret = Caller<CALL_ONCE_VI_VPSS_VO_Stop>::VI_VPSS_VO_Relate(_vi, _vpss, _vo);
    funcInfo(ret, "CALL_ONCE_VI_VPSS_VO_Stop");

    SAMPLE_COMM_SYS_Exit();
    sdk_exit();

    return 0;
}

template<typename T> void printInfo(T param, bool isNeedPause, int line)
{
    std::cout<<"\n########-line = " << line << "----" << param << "-########"<<std::endl;
    if (isNeedPause == true)
        getchar();
}

template<typename T> void funcInfo(bool ret, T param)
{
    if (ret == true)
        std::cout<<"\n########-FUNC = " << param << "----" << "TRUE-########"<<std::endl;
    else
        std::cout<<"\n########-FUNC = " << param << "----" << "FALSE-########"<<std::endl;
}

void frmMessage(const VideoProcSubSys& vpss,const VideoOutput& vo)
{
    IVE_IMAGE_S img;
    VIDEO_FRAME_INFO_S resizeFrm ,frm;
    memset_s(&frm, sizeof(VIDEO_FRAME_INFO_S), 0 ,sizeof(VIDEO_FRAME_INFO_S));

    HI_S32 s32MilliSec = 2000;
    VO_LAYER voLayer = vo.getParam()->voLayer;
    VO_CHN voChn = vo.getParam()->voChn;
    VPSS_GRP vpssGrp = vpss.getParam()->grpID;
    VPSS_CHN vpssChn = vpss.getParam()->channelConfig[vpss.getParam()->usedChannelNum-1].channelId;
    static OSD osd_LN, osd_key;

    //设置标志，控制当前识别模式
    static bool isNeedLandNumDetect = true;
    static bool isNeedKeyDetect = true;

    const int max_frm = 20;
    vector<bool> vecKeyFlag;
    vector<bool> vecLNFlag;

    while(stopSignal != true)
    {
        HI_S32 ret = HI_MPI_VPSS_GetChnFrame(vpssGrp, vpssChn, &frm, s32MilliSec);

        if(ret != HI_SUCCESS)
        {
            std::cout << "HI_MPI_VPSS_GetChnFrame == FALSE" << std::endl;
            ret = HI_MPI_VPSS_ReleaseChnFrame(vpssGrp, vpssChn, &frm );
            if(ret != HI_SUCCESS)
                std::cout << "HI_MPI_VPSS_ReleaseChnFrame == FALSE" << std::endl;
            printInfo("press enter to retry");
            continue;
        }

        MppFrmResize(&frm, &resizeFrm, 640, 384);
        FrmToOrigImg(&resizeFrm, &img);

        if(isNeedLandNumDetect == true) 
        {
            //辅助线检测
            bool laneFlag = false , numFlag = false;
            std::thread laneThread([&]{
                std::lock_guard<std::mutex> mtx_locker(mtx);
                laneFlag = processLaneInfo(
                    getLaneInfo(std::ref(frm), std::ref(resizeFrm), RGB888_RED),
                    uartID
                );
            });
            laneThread.join();

            //数字检测
            std::thread detectNumThread([&]{
                std::lock_guard<std::mutex> mtx_locker(mtx);
                numFlag = processNumInfo(
                    getDetectInfo(std::ref(detectNum), std::ref(img), std::ref(frm), RGB888_BLUE)
                );
            });
            detectNumThread.join();
            if(laneFlag == false && numFlag == false)
                vecLNFlag.push_back(false);
            else
                vecLNFlag.clear();

            if(vecLNFlag == vector<bool>(max_frm ,false))
            {
                cout<< "===============================change to key detection" <<endl;
                isNeedLandNumDetect = false;
                isNeedKeyDetect = true;
                vecLNFlag.clear();
                osd_LN.showStr(" ", 10, 10, RGB888_WHITE);
            }
        }

        if(isNeedKeyDetect == true)
        {
            bool keyFlag = false;
            //关键点检测
            std::thread detectKeyThread([&]{
                std::lock_guard<std::mutex> mtx_locker(mtx);
                keyFlag = processKeyInfo(
                    getDetectInfo(std::ref(detectKey), std::ref(img), std::ref(frm), RGB888_GREEN)
                );
            });
            detectKeyThread.join();

            if(keyFlag == false) 
                vecKeyFlag.push_back(false);
            else 
                vecKeyFlag.clear();
            
            if(vecKeyFlag == vector<bool>(max_frm + 10,false))
            {
                cout<< "===============================change to LaneNum detection" <<endl;
                isNeedKeyDetect = false;
                isNeedLandNumDetect = true;
                vecKeyFlag.clear();
                osd_key.showStr(" ", 10, 50, RGB888_WHITE);
            }
        }

        if(isNeedLandNumDetect == true)
        {
            osd_LN.showStr("current detected : lane and num", 10, 10);
        }
        if(isNeedKeyDetect == true)
        {
            osd_key.showStr("current detected : key detection", 10, 50);
        }
        
        //std::cout << "frmWidth==" << frm.stVFrame.u32Width << " | " << "frmHeight==" << frm.stVFrame.u32Height << std::endl;
        //std::cout << "imgWidth==" << img.u32Width << " | " << "imgHeight==" << img.u32Height << std::endl;

        ret = HI_MPI_VO_SendFrame(voLayer, voChn, &frm, 0);
        MppFrmDestroy(&resizeFrm);
        ret = HI_MPI_VPSS_ReleaseChnFrame(vpssGrp, vpssChn, &frm);
    }
}